#include "Camera.hpp"

#define GROUND_HEIGHT 2.0f
#define BASE_SPEED 10
#define PAN_RATE 2160.0f
#define TIME_STEP 0.01f
#define TIME_STEP_2 0.01f*0.01f
#define RADIAN 0.017453292519943295769f
#define GRAVITY 0.0f

Camera::Camera(){
  flyMode = false;

  wsvel = BASE_SPEED;
  advel = BASE_SPEED;
  hvel  = 0.0;
  inJump = false;

  wsdir = 0;
  addir = 0;  

  currentLocation = glm::vec3(0.0, GROUND_HEIGHT, 0.0);
  lastLocation = glm::vec3(0.0, GROUND_HEIGHT, 0.0);
  
  faceVector = glm::vec3(0.0);
  upVector = glm::vec3(0.0);
  rightVector = glm::vec3(0.0);
  

  dmx = 0;
  dmy = 0;
  
  viewYaw = 0.0;
  viewPitch = 0.0; 

  timeLeft = 0.0;
}

Camera::~Camera(){

}

void Camera::Update(float deltaT){
  timeLeft+=deltaT;

  viewYaw-=GLfloat(dmx)/screenWidth*PAN_RATE*deltaT;
  viewPitch-=GLfloat(dmy)/screenHeight*PAN_RATE*deltaT;
  viewPitch = glm::max(-90.0f,glm::min(90.0f,viewPitch));
  dmx = 0.0;
  dmy = 0.0;

  while(timeLeft>TIME_STEP){
    if(flyMode){
      /*translating spherical movement of yaw and pitch into cartesian world coordinate*/     
      
      currentLocation.x+=(addir*advel*cos(viewYaw*RADIAN)+wsdir*wsvel*sin(viewYaw*RADIAN)*cos(viewPitch*RADIAN))*TIME_STEP;
      currentLocation.y+= (wsdir*wsvel*sin(-viewPitch*RADIAN))*TIME_STEP;
      currentLocation.z+=(-wsdir*wsvel*cos(viewYaw*RADIAN)*cos(viewPitch*RADIAN)+addir*advel*sin(viewYaw*RADIAN))*TIME_STEP;
      
    } else {
      currentLocation.x+=(addir*advel*cos(viewYaw*RADIAN)+wsdir*wsvel*sin(viewYaw*RADIAN))*TIME_STEP;
      currentLocation.y += (TIME_STEP*hvel+0.5*-GRAVITY*TIME_STEP_2);
      currentLocation.z+=(-wsdir*wsvel*cos(viewYaw*RADIAN)+addir*advel*sin(viewYaw*RADIAN))*TIME_STEP;
      hvel -= TIME_STEP*GRAVITY;
    }

    lastLocation = currentLocation;      
    timeLeft-=TIME_STEP;
  }

  updateNearPlane();
}

void Camera::HandleEvent(sf::Event evt){
 
  switch (evt.Type) {
  case sf::Event::KeyPressed:
    {
      switch (evt.Key.Code){
	//movement
      case sf::Key::W:
	wsdir = 1;
	break;
      case sf::Key::S:
	wsdir = -1;
	break;
      case sf::Key::A:
	addir  = -1;
	break;
      case sf::Key::D:
	addir =  1;
	break;
      case sf::Key::Space:
	if(!inJump && !flyMode){
	  hvel = 10.0;
	  inJump = true;
	}
	break;
      case sf::Key::F: //debug
	flyMode = !flyMode;
	inJump = false;
	hvel = 0.0;
	break;
      case sf::Key::P: //debug
	printf("Camera location %.4f\t%.4f\t%.4f\n Camera Yaw %.4f\tPitch %.4f\n", currentLocation[0], currentLocation[1],  currentLocation[2], viewYaw, viewPitch);
	break;
      default:
	break;
      }
    }
    break;
  case sf::Event::KeyReleased:
    {
      switch (evt.Key.Code){
      case sf::Key::W:
      case sf::Key::S:
	wsdir = 0;
	break;
      case sf::Key::A:
      case sf::Key::D:
	addir = 0;
	break;
      default:
	break;
      }
    }
    break;
    //mouse control
  case sf::Event::MouseButtonPressed:
    break;
  case sf::Event::MouseMoved:
    if(windowInFocus){
      dmx = (screenWidth>>1) -  evt.MouseMove.X;
      dmy = (screenHeight>>1)  -  evt.MouseMove.Y;
    }
    break;
  case sf::Event::MouseButtonReleased:
    break;
    
  default:
    break;
  }
  
}

void Camera::Setup(){
  glRotatef(viewYaw, 0.0, 1.0, 0.0);
  glRotatef(viewPitch, 1.0*cos(viewYaw*RADIAN), 0.0, 1.0*sin(viewYaw*RADIAN));
  glTranslatef(-currentLocation.x,-currentLocation.y, -currentLocation.z);
}



void Camera::updateNearPlane(){
  
  faceVector[0] = sin(viewYaw*RADIAN)*cos(viewPitch*RADIAN);
  faceVector[1] = sin(-viewPitch*RADIAN);
  faceVector[2] = -cos(-viewYaw*RADIAN)*cos(viewPitch*RADIAN);

  faceVector = glm::normalize(faceVector);

  upVector[0] = sin((viewYaw-180)*RADIAN)*sin(-viewPitch*RADIAN);
  upVector[1] = cos(viewPitch*RADIAN);
  upVector[2] = -cos((viewYaw-180)*RADIAN)*sin(-viewPitch*RADIAN);
  upVector = glm::normalize(upVector);

  rightVector = glm::cross(faceVector,upVector);
  rightVector = glm::normalize(rightVector);


}


